/* Para resolver sistemas de ecuaciones, invertir matrices, 
 * descomponer matrices, y multiplicar matrices por vectores. */
#include "vector.h"
#include "matriz.h"
#include "sistemas.h"

void invertirTriangSup(Matriz& A, Matriz& res, int n) /* A triangular superior. */
{
	fornd(i, n)
	{
		fornd(j, i+1)
		{
			if (i == j)
			{
				res[i][j] = 1/A[i][j];
			}
			else
			{
				res[i][j] = -res[i][i] * A[i][j];
			}
		}
		
		forn(j, i)
		{
			res[i][j] = 0;
		}
	}
	
	return;
}

void invertirTriangInf(Matriz& A, Matriz& res, int n) /* A triangular inferior con ceros en la diagonal. */
{
	fornd(i, n)
	{
		forn(j, i)
		{
			if (i == j)
			{
				res[i][j] = 1;
			}
			else
			{
				res[i][j] = -res[i][i] * A[i][j];
			}	
		}
		
		fornx(j, i+1, n)
		{
			res[i][j] = 0;			
		}
	}
	
	return;
}

void matrizPorVector(Matriz& A, Vector& x, Vector& res, int n) /* res = A.x */
{
	forn(i, n)
	{
		double aux = 0;
		
		forn(j, n)
		{
			aux += A[i][j] * x[j];
		}
		
		res[i] = aux;
	}
	
	return;	
}

void matrizPorDiagonal(Matriz &A, Vector &d, Matriz& res, int n) 
/* res = A.B donde B[i][j] = o si i != j, d[i] si i == j. */
{
	forn(i, n)
	{
		forn(j, n)
		{
			res[i][j] = d[j] * A[i][j];			
		}	
	}
	
	return;	
}

void diagonalPorMatriz(Vector &d, Matriz& A, Matriz& res, int n) 
/* res = B.A donde B[i][j] = o si i != j, d[i] si i == j. */
{
	forn(i, n)
	{
		forn(j, n)
		{		
			res[i][j] = d[i] * A[i][j];			
		}	
	}
	
	return;	
}

void gramSchmidt(Matriz& A, Matriz& R, Matriz& Q, int n) /* Devuelve Q traspuesta*/
{
	Matriz aux (n, Vector(n));
	Matriz u (n, Vector(n));	
	Vector uaux (n);
	
	traspuesta(A, aux, n);
	
	/* Columnas de A. */
	forn(i, n)
		u[i] = aux[i];	

	/* Columna j -= proyecciones sobre cada elemento de la base u. */
	/* u_k = v_k - sum_{j=1}^{k-1} proj_{u_j} v_k */
	
	fornx(j, 1, n)
	{
		/* Uso la base que tengo hasta ahora. */
		fornx(k, 1, j)
		{
			proyeccion(u[k], u[j], uaux, n);
			resta(u[j], uaux, n);
		}
	}
	
	/* Normalizo. */
	forn(i, n)
		normalizar(u[i], n);
	
	/* R. */
	forn(i, n)
	{
		fornx(j, i, n)
		{
			R[i][j] = productoInterno(u[i], aux[j], n);
		}
		
		fornx(j, 0, i)
		{
			R[i][j] = 0;
		}
	}
	
	/* Q = e. */
	forn(i, n)
		Q[i] = u[i];
		
	return;
}

bool descomposicionPLU(Matriz& A, Vector& Pi, Matriz& res, int n) /* Pi[i] = j <-> P[i][j] = 1. */
/* res contiene L y U a la vez. Devuelve falso si la matríz no es inversible. */
{
	double p, k2;
		
	forn(i, n)
		res[i] = A[i];
	
	forn(i, n)
		Pi[i] = i;	
	
	forn(k, n-1)
	{
		p  = 0;		
		k2 = 0;		
		
		fornx(i, k, n)
		{
			if (fabs(res[i][k]) > p)
			{
				p = fabs(res[i][k]);			
				k2 = i;
			}
		}
		
		if (p == 0)
		{
			//~ cout << "La matríz es no inversible" << endl;
			return false;
		}
		
		swap(Pi, k, k2);		
		
		/* Para pivotear. */
		forn(i, n)
			swap(res, k, k2, i, n);
		
		fornx(i, k+1, n)
		{
			res[i][k] = res[i][k]/res[k][k];
			
			fornx(j, k+1, n)
				res[i][j] = res[i][j] - res[i][k]*res[k][j];
		}
	}
		
	return true;
}

void backwardSubs(Matriz& L, Vector& Pi, Vector& b, Vector& x, int n) 
/* Resuelve el sistema triangular inferior Lx = Pb donde P es una matríz de permutación */
{
	//~ x_i = b_{Pi[i]} - \sum_{j=0}^{i-1} L[j][i]*x_j
	
	forn(i, n)
		x[i] = b[Pi[i]];		
	
	forn(i, n)
	{
		forn(j, i-1)
			x[i] -= L[j][i] * x[j]; 
	}
	
	return;
}

void forwardSubs(Matriz& U, Vector& Pi, Vector& b, Vector& x, int n) 
/* Resuelve el sistema triangular superior Ux = Pb donde P es una matríz de permutación */
{
	//~ x_i = (b_{Pi[i]} - \sum_{j=i+1}^{n-1} U[i][j]*x_j) / u[i][i]
	
	forn(i, n)
		x[i] = b[Pi[i]];		
	
	fornd(i, n)
	{
		if (i < n - 1)
		{
			fornx(j, i+1, n)
				x[i] -= U[i][j] * x[j]; 
		}
		
		if (U[i][i] > 1e-6)
			x[i] /= U[i][i];
	}
	
	return;
}

void intercambiarFilas(Matriz& A, Vector& v, Matriz& res, int n)
{
	forn(i, n)	
		res[i] = A[v[i]];	
}

void solvePLU(Matriz& M, Vector& d, Vector& aproximacion, int n)
{
	/* LU*x = P*d, U*x = L^(-1)P*d */
	
	Matriz res (n, Vector(n));
	Vector pi (n);
	descomposicionPLU(M, pi, res, n);
	
	/* inversa de L */
	Matriz invL (n, Vector(n));
	invertirTriangInf(res , invL, n);
	/* L^-1*P */
	Matriz aux (n, Vector(n));
	intercambiarFilas(invL, pi, aux, n);
	
	/* L^-1*P*d */
	Vector b (n);
	matrizPorVector(aux, d, b, n);	
	
	/* U x = L^-1*P*d */
	forwardSubs(res, pi, b, aproximacion, n);	
	
	/* Acá se tiene la aproximación de la posición del enemigo. */		
	
	return;
}
void solvePLUPivEsc(Matriz& M_enemiga, Vector& impacto_enemigo, Vector& pos_enemiga, int n)
{
	Vector maximos(n, 0);
	Vector Pi(n);

	/* Ahora miro el maximo de cada fila y lo dejo en el vector maximos*/
	forn(i, n)
	{		
		forn(j, n)
		{
			if(fabs(maximos[i]) < fabs(M_enemiga[i][j]))
			{
				maximos[i] = fabs(M_enemiga[i][j]);
			}	
		}
	}
	
	forn(i, n)
	{
		int p = 0;
		int max = 0;
		
		forn(j, n)
		{
			if(maximos[i] == 0)
			{
				
			}else{
				int aux = fabs(M_enemiga[i][j]) /maximos[i];
				if (aux > max) 
				{
					max = aux;
					p = j;	
				}
			}
		}
		
		Vector aux_swap = M_enemiga[i];
		M_enemiga[i] = M_enemiga[p];
		M_enemiga[p] = aux_swap;
		swap(impacto_enemigo, i, p);
		
		Pi[i] = p;
		
		double pivote = M_enemiga[i][i];
		fornx(j, i +1, n)
		{
			if(M_enemiga[j][i] != 0)
			{
				double beta = M_enemiga[j][i]/ pivote;
				fornx(k, i, n)
				{
					M_enemiga[j][k] -= beta*M_enemiga[i][k];
				}
				impacto_enemigo[j] -= beta*impacto_enemigo[i];	
			} 
		}
	}
	
	forwardSubs(M_enemiga, Pi, impacto_enemigo, pos_enemiga, n);
	
	return;
}
void solveQR(Matriz& M, Vector& d, Vector& aproximacion, int n)
{
	Matriz R (n, Vector(n));
	Matriz Q (n, Vector(n));
	gramSchmidt(M, R, Q, n); /* QR x = d, R x = Q^t d */
	
	Vector b (n);
	matrizPorVector(Q, d, b, n); /* R x = b */
	/* Como R es una matríz triangular superior se puede usar el método 
	 * forwardsub para resolver el sistema.
	 * Como este método recibe una vector que representa una matríz de 
	 * permutación y en este caso no hay permutación, el vector P es 
	 * tal que P[i] = i. */
	
	Vector Pi (n);
	forn(i, n)
		Pi[i] = i;
	forwardSubs(R, Pi, b, aproximacion, n);
	
	return;
}

void matrizInicial(Matriz& A, Matriz& hil, Vector& pos, Vector& x, int n)
{		
	/* Generar posición random como aproximación del enemigo. */	
	generarVector(pos, n);	
	buscarMatriz(A, hil, pos, x, n);
	
	return;
}

void Hilbert(Matriz& A, int n)
/* Calcula una matríz de Hilbert de nxn elementos */
{
	forn(i, n)
	{
		forn(j, n)
		{
			A[i][j] = 1.0/(i+j+1);
			/* Porque en la definición empiezan a contar desde 1, entonces quedaría 
			 * (i+1) + (j+1) - 1 = i+j+1  */
		}
	}
}

/* matriz / matrix*x = aproximacion 
 * matriz[i][i] = aproximacion[i]/x[i] */
void buscarMatriz(Matriz& matriz, Matriz& hil, Vector& y, Vector& x, int n)
{
	/* diag*hilbert*x = y 
	 * diag*(hilbert*x) = y*/
	Vector xAux (n, 0);	
	matrizPorVector(hil, x, xAux, n);
	 
	/* Llenar los elementos de la diagonal, se puede hacer con un vector. */
	Vector diag (n);	
	forn(i, n)
	{
		if (y[i] <= 1e-6 || xAux[i] <= 1e-6)
			diag[i] = 0.000001;
		else
			diag[i] = y[i] / xAux[i];
	}
	diagonalPorMatriz(diag, hil, matriz, n);
		
	return;
}
